//
//  AccelerometerFilter.swift
//  HSMainViewControllerHandler
//
//  Created by haisheng huang on 16/7/20.
//  Copyright © 2016年 haisheng huang. All rights reserved.
//

import Foundation
import CoreMotion
import UIKit

let kAccelerometerMinStep: Double = 0.02
let kAccelerometerNoiseAttenuation: Double = 3.0

open class AccelerometerFilter: NSObject {

    var adaptive: Bool?
    var x: UIAccelerationValue?
    var y: UIAccelerationValue?
    var z: UIAccelerationValue?
    
    open func Nrom(_ x: Double, y: Double, z: Double) -> Double {
        return sqrt(x * x + y * y + z * z)
    }
    
    open func Clamp(_ v : Double, min: Double, max: Double) -> Double {
        if v > max {
            return max
        } else if v < min {
            return min
        } else {
            return v
        }
    }
    
    func addAcceleration(_ accel: CMAcceleration) -> Void {
        
        x = accel.x
        y = accel.y
        z = accel.z
        
    }
    
}


class LowpassFilter: AccelerometerFilter {

    var filterConstant: Double?
    var lastX: UIAccelerationValue?
    var lastY: UIAccelerationValue?
    var lastZ: UIAccelerationValue?
    
    override init() {
        super.init()
    }
    
    convenience init(sampleRate: Double, cutoffFrequency: Double) {
        
        self.init()
        
        let dt = 1.0 / sampleRate
        let RC = 1.0 / cutoffFrequency
        
        filterConstant = dt / (dt + RC)
        
    }
    
    override func addAcceleration(_ accel: CMAcceleration) {
        
        var alpha = filterConstant;
        
        if (adaptive == true) {
            let d = Clamp(fabs(Nrom(x!, y: y!, z: z!) - Nrom(accel.x, y: accel.y, z: accel.z)) / kAccelerometerMinStep - 1.0, min: 0.0, max: 1.0)
            alpha = (1.0 - d) * filterConstant! / kAccelerometerNoiseAttenuation + d * filterConstant!
        }
        
        x = accel.x * alpha! + x! * (1.0 - alpha!)
        y = accel.y * alpha! + y! * (1.0 - alpha!)
        z = accel.z * alpha! + z! * (1.0 - alpha!)
        
    }
}


class HighpassFilter: AccelerometerFilter {

    var filterConstant: Double?
    var lastX: UIAccelerationValue?
    var lastY: UIAccelerationValue?
    var lastZ: UIAccelerationValue?
    
    override init() {
        super.init()
    }
    
    convenience init(sampleRate: Double, cutoffFrequency: Double) {
        
        self.init()
        
        let dt = 1.0 / sampleRate
        let RC = 1.0 / cutoffFrequency
        
        filterConstant = RC / (dt + RC)
        
    }

    override func addAcceleration(_ accel: CMAcceleration) {
        
        var alpha = filterConstant
        
        if adaptive == true {
            
            let d = Clamp(fabs(Nrom(x!, y: y!, z: z!) - Nrom(accel.x, y: accel.y, z: accel.z) / kAccelerometerMinStep - 1.0), min: 0.0, max: 1.0)
            alpha = d * filterConstant! / kAccelerometerNoiseAttenuation + (1.0 - d) * filterConstant!
        }
        
        x = alpha! * (x! + accel.x - lastX!)
        y = alpha! * (y! + accel.y - lastY!)
        z = alpha! * (z! + accel.z - lastZ!)
        
        lastX = accel.x
        lastY = accel.y
        lastZ = accel.z
    }
}



